#include "../User/BSP/spi.h"

#include "main.h"

const uint8_t READWRITE_CMD = 0x80;

extern SPI_HandleTypeDef hspi1;

void BSP_SPI_Activited() {
  HAL_GPIO_WritePin(SPI1_NCS_GPIO_Port, SPI1_NCS_Pin, GPIO_PIN_RESET);
}

void BSP_SPI_UNActivited() {
  HAL_GPIO_WritePin(SPI1_NCS_GPIO_Port, SPI1_NCS_Pin, GPIO_PIN_SET);
}

uint8_t BSP_SPI_WriteRead(uint8_t Byte) {
  uint8_t receivedbyte = 0;
  // TODO: CHANGE TIMEOUT
  if (HAL_SPI_TransmitReceive(&MPU6500_SPI, (uint8_t*)&Byte,
                              (uint8_t*)&receivedbyte, 1, 0x1000) != HAL_OK) {
    return 0xff;
  } else {
  }
  return receivedbyte;
}

void BSP_SPI_Write(uint8_t* pBuffer, uint8_t WriteAddr,
                   uint16_t NumByteToWrite) {
  BSP_SPI_Activited();
  BSP_SPI_WriteRead(WriteAddr);
  while (NumByteToWrite >= 0x01) {
    BSP_SPI_WriteRead(*pBuffer);
    NumByteToWrite--;
    pBuffer++;
  }
  BSP_SPI_UNActivited();
}

void BSP_SPI_Read(uint8_t* pBuffer, uint8_t ReadAddr, uint16_t NumByteToRead) {
  BSP_SPI_Activited();
  uint8_t data = ReadAddr | READWRITE_CMD;
  HAL_SPI_Transmit(&MPU6500_SPI, &data, 1, HAL_MAX_DELAY);
  HAL_SPI_Receive(&MPU6500_SPI, pBuffer, NumByteToRead, HAL_MAX_DELAY);
  BSP_SPI_UNActivited();
}
